#include "gl_data.hpp"

namespace gl
{
    /*
    void set_line(GLfloat** vert, const Vec3d& start, const Vec3d& end)
    {
        *((*vert)) = start.x;
        *((*vert)++) = start.y;
        *((*vert)++) = start.z;
        *((*vert)++) = end.x;
        *((*vert)++) = end.y;
        *((*vert)++) = end.z;
        (*vert)++;
    }
    */

    void make_line_vert(GLfloat* vert, int idx, const Vec3d& start, const Vec3d& end)
    {
        vert[idx] = start.x;
        vert[idx + 1] = start.y;
        vert[idx + 2] = start.z;
        vert[idx + 3] = end.x;
        vert[idx + 4] = end.y;
        vert[idx + 5] = end.z;
    }

    void box2_data(const Vec3d& pos_min, const Vec3d& dimen, GLData& data, const Rgb& color)
    {
        std::array<Vec3d, 8> box;
        pt::point2_box(pos_min, dimen, box);

        data.size_pos = 24;
        data.size_color = 12;
        data.len_pos = 3;
        data.len_color = 3;
        data.mode = GL_LINES;

        data.pos_min.x = pos_min.x;
        data.pos_min.y = pos_min.y;
        data.pos_min.z = pos_min.z;

        data.pos_max.x = pos_min.x + dimen.x;
        data.pos_max.y = pos_min.y + dimen.y;
        data.pos_max.z = pos_min.z + dimen.z;

        data.colors = (GLfloat*)calloc(data.len_color * data.size_color, sizeof(GLfloat));
        data.vertices = (GLfloat*)calloc(data.len_pos * data.size_pos, sizeof(GLfloat));

        GLfloat* vert_idx = data.vertices;
        gl::make_line_vert(vert_idx, 0, box[0], box[1]);
        gl::make_line_vert(vert_idx, 6, box[1], box[2]);
        gl::make_line_vert(vert_idx, 12, box[2], box[3]);
        gl::make_line_vert(vert_idx, 18, box[3], box[0]);
        gl::make_line_vert(vert_idx, 24, box[4], box[5]);
        gl::make_line_vert(vert_idx, 30, box[5], box[6]);
        gl::make_line_vert(vert_idx, 36, box[6], box[7]);
        gl::make_line_vert(vert_idx, 42, box[7], box[4]);
        gl::make_line_vert(vert_idx, 48, box[0], box[4]);
        gl::make_line_vert(vert_idx, 54, box[1], box[5]);
        gl::make_line_vert(vert_idx, 60, box[2], box[6]);
        gl::make_line_vert(vert_idx, 66, box[3], box[7]);
        /*
        set_line(&vert_idx, box[0], box[1]);
        set_line(&vert_idx, box[1], box[2]);
        set_line(&vert_idx, box[2], box[3]);
        set_line(&vert_idx, box[3], box[0]);

        set_line(&vert_idx, box[4], box[5]);
        set_line(&vert_idx, box[5], box[6]);
        set_line(&vert_idx, box[6], box[7]);
        set_line(&vert_idx, box[7], box[4]);

        set_line(&vert_idx, box[0], box[4]);
        set_line(&vert_idx, box[1], box[5]);
        set_line(&vert_idx, box[2], box[6]);
        set_line(&vert_idx, box[3], box[7]);
        */
        int tp_idx = 0;
        for (int i = 0; i < 24; ++i)
        {
            tp_idx = i * 3;
            //std::cout << data.vertices[tp_idx] << ", " << data.vertices[tp_idx + 1] << ", " << data.vertices[tp_idx + 2] << std::endl;
        }
        int idx = 0;
        for (int i = 0; i < 12; ++i)
        {
            idx = i * 3;
            data.colors[idx] = color.r;
            data.colors[idx + 1] = color.g;
            data.colors[idx + 2] = color.b;
        }
    }

    void cloud2_data(const PointCloud& cloud, GLData& data, const Rgb* def_color)
    {
        int index = 0;
        int length = 3;

        int pt_size = cloud.points.size();
        bool has_color = cloud.colors.size() == pt_size ? true : false;

        data.size_pos = pt_size;
        data.size_color = pt_size;
        data.len_pos = length;
        data.len_color = length;
        data.mode = GL_POINTS;
        data.pos_max = cloud.point_max;
        data.pos_min = cloud.point_min;

        data.colors = (GLfloat*)calloc(data.len_color * data.size_color, sizeof(GLfloat));
        data.vertices = (GLfloat*)calloc(data.len_pos * data.size_pos, sizeof(GLfloat));

        for (int i = 0; i < pt_size; ++i)
        {
            index = i * length;
            if (has_color) {
                data.colors[index] = cloud.colors[i].r;
                data.colors[index + 1] = cloud.colors[i].g;
                data.colors[index + 2] = cloud.colors[i].b;
            }
            else if (nullptr != def_color) {
                data.colors[index] = def_color->r;
                data.colors[index + 1] = def_color->g;
                data.colors[index + 2] = def_color->b;
            }
            else {
                data.colors[index] = 1.0f;
                data.colors[index + 1] = 1.0f;
                data.colors[index + 2] = 1.0f;
            }

            data.vertices[index] = cloud.points[i].x;
            data.vertices[index + 1] = cloud.points[i].y;
            data.vertices[index + 2] = cloud.points[i].z;
        }
    }
}

GLData::GLData()
    :size_pos(0)
    , size_color(0)
    , len_pos(0)
    , len_color(0)
    , colors(nullptr)
    , vertices(nullptr)
{

}

GLData::GLData(const std::string key_,enum Sort sort_)
    :size_pos(0)
    , size_color(0)
    , len_pos(0)
    , len_color(0)
    , key(key_)
    , sort(sort_)
    , colors(nullptr)
    , vertices(nullptr)
{

}

GLData::~GLData()
{
    clear();
}

void GLData::clear()
{
    if (nullptr != vertices)
    {
        delete[] vertices;
        vertices = nullptr;
    }
    if (nullptr != colors)
    {
        delete[] colors;
        colors = nullptr;
    }
    size_pos = 0;
    size_color = 0;
    len_pos = 0;
    len_color = 0;
}